#include "postprocess/processor/base_processor.h"

#include "postprocess/common/common_func.h"
#include "postprocess/common/postprocess_id_generator.h"

BaseProcessor::BaseProcessor() noexcept {
  class_vec_.clear();
  // tracker_prediction_objects_.clear();
}

BaseProcessor::~BaseProcessor() {}

void BaseProcessor::SetClassFlag(const std::vector<uint32_t> &vec) noexcept {
  class_vec_.clear();
  class_vec_.assign(vec.begin(), vec.end());
}

void BaseProcessor::AddClassFlag(uint32_t flag) noexcept {
  class_vec_.emplace_back(flag);
}

void BaseProcessor::AddAndClearClassFlag(uint32_t flag) noexcept {
  class_vec_.clear();
  class_vec_.emplace_back(flag);
}

void BaseProcessor::transform_tracker_from_bbox(
    TrackedObjectPtr *tracker, const BoundingBox &bbox) noexcept {
  (*tracker)->center =
      Eigen::Vector3f{bbox.center.x, bbox.center.y, bbox.center.z};
  (*tracker)->size = Eigen::Vector3f{bbox.length, bbox.width, bbox.height};
  (*tracker)->heading = bbox.heading;
  (*tracker)->r = bbox.r;
  (*tracker)->score = bbox.score;
  (*tracker)->label = bbox.label;
  (*tracker)->polygon = bbox.polygon;
  for (int i = 0; i < 10; ++i) {
    (*tracker)->scores[i] = bbox.scores[i];
  }
  // initialized,to be determined
  (*tracker)->id = 0;
  (*tracker)->velocity = Eigen::Vector3f{bbox.vx, bbox.vy, 0.0f};
  (*tracker)->acceleration = Eigen::Vector3f{bbox.ax, bbox.ay, 0.0f};
  (*tracker)->association_score = 0.0f;
}

void BaseProcessor::transform_tracker_from_bbox(
    TrackedObjectPtr *tracker, const BoundingBox &bbox,
    const Eigen::Matrix4f &transformed_matrix) noexcept {
  Eigen::Vector4f cen =
      transformed_matrix *
      Eigen::Vector4f{bbox.center.x, bbox.center.y, bbox.center.z, 1.0f};

  (*tracker)->center = Eigen::Vector3f{cen.x(), cen.y(), cen.z()};
  (*tracker)->size = Eigen::Vector3f{bbox.length, bbox.width, bbox.height};
  (*tracker)->heading = bbox.heading;
  (*tracker)->r = bbox.r;
  (*tracker)->score = bbox.score;
  (*tracker)->label = bbox.label;
  for (int i = 0; i < 10; ++i) {
    (*tracker)->scores[i] = bbox.scores[i];
  }

  (*tracker)->polygon.clear();
  for (int i = 0; i < bbox.polygon.size(); ++i) {
    auto point = bbox.polygon.at(i);
    Eigen::Vector4f p = transformed_matrix *
                        Eigen::Vector4f{point.x, point.y, bbox.height, 1.0f};
    autodrive::perception::Point2f poly;
    poly.x = p.x();
    poly.y = p.y();
    (*tracker)->polygon.add(poly);
  }
  (*tracker)->polygon = bbox.polygon;
  // initialized,to be determined
  (*tracker)->id = 0;
  (*tracker)->velocity = Eigen::Vector3f{bbox.vx, bbox.vy, 0.0f};
  (*tracker)->acceleration = Eigen::Vector3f{bbox.ax, bbox.ay, 0.0f};
  (*tracker)->association_score = 0.0f;
}

void BaseProcessor::predict(
    const autodrive::perception::PerceptionResult &percetion,
    double time_diff) noexcept {
  // tracker_prediction_objects_.clear();
  u_int32_t tracker_num = percetion.tracker_objects.size();
  for (auto i = 0; i < tracker_num; i++) {
  }
}

void BaseProcessor::construct_tracked_objects(
    const autodrive::perception::fixed_array<BoundingBox, kMaxBoundingBoxNum>
        &dl_boxes,
    std::vector<TrackedObjectPtr> *tracked_objects,
    std::uint64_t timestamp) noexcept {
  if (class_vec_.empty()) return;
  uint32_t num_dl_boxes = dl_boxes.size();
  uint32_t num_tracker = 0;
  // 获取class_vec_其中包含的类别的数量
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.at(i);
    if (PostProcessCommon::IsContained(class_vec_, bbox.label)) {
      num_tracker++;
    }
  }
  // 清空和重置tracker
  tracked_objects->clear();
  tracked_objects->resize(num_tracker);
  uint32_t index = 0;
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.at(i);
    if (PostProcessCommon::IsContained(class_vec_, bbox.label)) {
      TrackedObjectPtr tracker = std::make_shared<TrackedObject>();
      transform_tracker_from_bbox(&tracker, bbox);
      tracker->timestamp = timestamp;
      tracker->processor_flag = processor_flag_;
      (*tracked_objects)[index] = tracker;
      index++;

      // std::cout
      //     <<
      //     "----------------construct_tracked_objects-----------------------"
      //     << std::endl;
      // std::cout << "tracker center " << tracker->center.x() << " "
      //           << tracker->center.y() << " " << tracker->center.z()
      //           << " size = " << tracker->size.x() << " " <<
      //           tracker->size.x()
      //           << " " << tracker->size.x() << std::endl;
      // std::cout << "-------------------------------------------------"
      //           << std::endl;
    }
  }
}

void BaseProcessor::construct_tracked_objects(
    const autodrive::perception::fixed_array<BoundingBox, kMaxBoundingBoxNum>
        &dl_boxes,
    std::vector<TrackedObjectPtr> *tracked_objects, uint64_t timestamp,
    const Eigen::Matrix4f &transformed_matrix) noexcept {
  if (class_vec_.empty()) return;
  uint32_t num_dl_boxes = dl_boxes.size();
  uint32_t num_tracker = 0;
  // 获取class_vec_其中包含的类别的数量
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.at(i);
    if (PostProcessCommon::IsContained(class_vec_, bbox.label)) {
      num_tracker++;
    }
  }

  // 清空和重置tracker
  tracked_objects->clear();
  tracked_objects->resize(num_tracker);
  uint32_t index = 0;
  for (uint32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.at(i);
    if (PostProcessCommon::IsContained(class_vec_, bbox.label)) {
      TrackedObjectPtr tracker = std::make_shared<TrackedObject>();
      transform_tracker_from_bbox(&tracker, bbox, transformed_matrix);
      tracker->timestamp = timestamp;
      tracker->processor_flag = processor_flag_;
      (*tracked_objects)[index] = tracker;
      index++;
    }
  }
}

void BaseProcessor::classify_tracked_objects(
    const BoundingBox &bbox, uint32_t &num_vehicle_tracker,
    uint32_t &num_pedestrain_tracker, uint32_t &num_other_tracker) noexcept {
  switch (bbox.label) {
    case 1:  // car
      num_vehicle_tracker++;
      break;
    case 2:  // truck
      num_vehicle_tracker++;
      break;
    case 3:  // construction_vehicle
      num_vehicle_tracker++;
      break;
    case 4:  // bus
      num_vehicle_tracker++;
      break;
    case 5:  // trailer
      num_vehicle_tracker++;
      break;
    case 6:  // barrier
      num_other_tracker++;
      break;
    case 7:  // motorcycle
      break;
    case 8:  // bicycle
      break;
    case 9:  // pedestrian
      num_pedestrain_tracker++;
      break;
    case 10:  // traffic_cone
      num_other_tracker++;
      break;
    default:
      break;
  }
}

void BaseProcessor::range(
    const autodrive::perception::fixed_array<BoundingBox, kMaxBoundingBoxNum>
        &dl_boxes,
    std::vector<std::int32_t> &objects_index) noexcept {
  objects_index.clear();
  uint32_t num_dl_boxes = dl_boxes.size();
  int insert_index = 0;
  for (std::int32_t i = 0; i < num_dl_boxes; i++) {
    const BoundingBox &bbox = dl_boxes.at(i);
    if (PostProcessCommon::IsContained(class_vec_, bbox.label)) {
      objects_index.push_back(insert_index);
      insert_index++;
    }
  }
}

void BaseProcessor::compute_tracks_predict(
    std::vector<Eigen::VectorXf> &tracks_predict, double time_diff,
    ObjectTrackSet &object_tracks_set) {
  std::int32_t num_track = object_tracks_set.size();
  tracks_predict.resize(num_track);
  std::vector<ObjectTrackPtr> &tracks = object_tracks_set.GetTracks();
  for (std::int32_t i = 0; i < num_track; ++i) {
    tracks_predict[i] = tracks[i]->Predict(time_diff);
  }
}

void BaseProcessor::create_new_tracks(
    std::vector<TrackedObjectPtr> const &new_objects,
    std::vector<std::int32_t> const &unassigned_objects_index,
    ObjectTrackSet &object_tracks_set) noexcept(false) {
  auto &id_generator = PostProcessIdGenerator::GetInstance();
  for (int i = 0; i < unassigned_objects_index.size(); i++) {
    std::int32_t obj_id{unassigned_objects_index.at(i)};
    ObjectTrackPtr track(new ObjectTrack(new_objects[obj_id]));
    std::uint16_t id = id_generator.GetId();
    track->SetID(id);
    object_tracks_set.AddTrack(track);
  }
}

void BaseProcessor::create_new_tracks(
    std::vector<TrackedObjectPtr> const &new_objects,
    std::vector<std::int32_t> const &unassigned_objects_index,
    ObjectTrackSet &object_tracks_set,
    PostProcessorSettings const &settings) noexcept(false) {
  auto &id_generator = PostProcessIdGenerator::GetInstance();
  for (int i = 0; i < unassigned_objects_index.size(); i++) {
    std::int32_t obj_id{unassigned_objects_index.at(i)};
    ObjectTrackPtr track(new ObjectTrack(new_objects[obj_id], settings));
    std::uint16_t id = id_generator.GetId();
    track->SetID(id);
    object_tracks_set.AddTrack(track);
  }
}

void BaseProcessor::update_assigned_tracks(
    std::vector<TrackedObjectPtr> *new_objects,
    const std::vector<TrackObjectPair> &assignments, double time_diff,
    ObjectTrackSet &object_tracks_set) {
  std::vector<ObjectTrackPtr> &tracks = object_tracks_set.GetTracks();
  for (size_t i = 0; i < assignments.size(); i++) {
    std::int32_t track_id = assignments[i].first;
    std::int32_t obj_id = assignments[i].second;
    tracks[track_id]->UpdateWithObject(&(*new_objects)[obj_id], time_diff);
  }
}

void BaseProcessor::update_unassigned_tracks(
    const std::vector<Eigen::VectorXf> &tracks_predict,
    const std::vector<int32_t> &unassigned_tracks, double time_diff,
    ObjectTrackSet &object_tracks_set) {
  std::vector<ObjectTrackPtr> &tracks = object_tracks_set.GetTracks();
  for (size_t i = 0; i < unassigned_tracks.size(); ++i) {
    std::int32_t track_id = unassigned_tracks[i];
    tracks[track_id]->UpdateWithoutObject(tracks_predict[track_id], time_diff);
  }
}

void BaseProcessor::delete_lost_tracks(
    ObjectTrackSet &object_tracks_set) noexcept(false) {
  object_tracks_set.RemoveLostTracks();
}
